from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from tf_transformations import quaternion_from_euler
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator
from rclpy.time import Time, Duration
from autopatol_interfaces.srv import SpeechText
import cv2 
from cv_bridge import CvBridge
from sensor_msgs.msg import Image

class PartolNode(BasicNavigator):
    def __init__(self, node_name='partol_node'):
        super().__init__(node_name)
        self.declare_parameter('init_pose', [0.0, 0.0 ,0.0])
        self.declare_parameter('target_points', [2.0, 1.0 ,0.0, 2.0, 2.0, 1.57, 0.0, 0.0 ,0.0])
        self.declare_parameter('image_save_path', '')
        self.init_pose = self.get_parameter('init_pose').value
        self.target_points = self.get_parameter('target_points').value
        self.image_save_path = self.get_parameter('image_save_path').value
        self.buffer = Buffer()
        self.listener = TransformListener(self.buffer, self)
        self.speaker_client = self.create_client(SpeechText, 'speech_text')
        self.latest_image = None
        self.bridge = CvBridge()
        self.image_sub = self.create_subscription(Image, '/camera_sensor/image_raw', self.image_callback,1)
    
    def image_callback(self, msg):
        self.latest_image = msg

    def record_image(self):
        if self.latest_image is not None:
            pose = self.get_current_pose()
            cv_image = self.bridge.imgmsg_to_cv2(self.latest_image)
            cv2.imwrite(
                f'{self.image_save_path}_img_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png', 
                cv_image)
            
            self.get_logger().info(f'保存图片到{self.image_save_path}')
        else:
            self.get_logger().warn('没有获取到图片数据')

    def get_pose_by_xyyaw(self, x, y, yaw):
        pose = PoseStamped()
        pose.header.frame_id = 'map'
        pose.pose.position.x = x
        pose.pose.position.y = y
        quat = quaternion_from_euler(0.0, 0.0, yaw)
        pose.pose.orientation.x = quat[0]
        pose.pose.orientation.y = quat[1]
        pose.pose.orientation.z = quat[2]
        pose.pose.orientation.w = quat[3]
        return pose
    
    def init_robot_pose(self):
        self.init_pose = self.get_parameter('init_pose').value
        init_pose = self.get_pose_by_xyyaw(self.init_pose[0], self.init_pose[1], self.init_pose[2])
        self.setInitialPose(init_pose)
        self.waitUntilNav2Active()

    def get_target_points(self):
        target_poses = []
        self.target_points = self.get_parameter('target_points').value
        for index in range(int(len(self.target_points)/3)):
            x = self.target_points[index*3]
            y = self.target_points[index*3+1]
            yaw = self.target_points[index*3+2]
            target_poses.append([x, y, yaw])
            self.get_logger().info(f'获取到目标点P{index}-> {x}, {y}, {yaw}')
        return target_poses
    
    def nav_to_pose(self,target_point):
        self.goToPose(target_point)
        while not self.isTaskComplete():
            feedback = self.getFeedback()
            self.get_logger().info(f'剩余距离:{feedback.distance_remaining}')
        res = self.getResult()
        self.get_logger().info(f'导航结果:{res}')
    
    def get_current_pose(self):
         while rclpy.ok():
            try:
                tf = self.buffer.lookup_transform(
                'map', 'base_footprint', Time(), Duration(seconds=1))
                transform = tf.transform
                self.get_logger().info(f'平移:{transform.translation}')
                return transform
            except Exception as e:
                self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')
    
    def speech_text(self, text):
        request = SpeechText.Request()
        request.text = text
        while not self.speaker_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().warn('SpeechText 服务不可用，等待中...')
        future = self.speaker_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        if future.result() is not None:
            response = future.result()
            if response.result == True:
                self.get_logger().info(f'语音播报结果: {future.result().result}')
            else:
                self.get_logger().error('语音播报失败')
        else:
            self.get_logger().error('语音播报服务响应失败')


def main():
    rclpy.init()
    partol_node = PartolNode()
    # rclpy.spin(partol_node)
    partol_node.speech_text('正在初始化机器人位置')
    partol_node.init_robot_pose()
    partol_node.speech_text('机器人位置初始化完成')
    
    while rclpy.ok():  
        target_poses = partol_node.get_target_points()
        for target_pose in target_poses:
            target_point = partol_node.get_pose_by_xyyaw(target_pose[0], target_pose[1], target_pose[2])
            partol_node.speech_text(f'正在前往目标点{target_pose[0]}, {target_pose[1]}')
            partol_node.nav_to_pose(target_point)
            partol_node.speech_text(f'已经到达目标点{target_pose[0]}, {target_pose[1]}，准备记录图像')
            partol_node.record_image()
            partol_node.speech_text(f'图像记录完成')
    rclpy.shutdown()